/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Ranging interface Server part implementation.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "RangingServer.hpp"
#include "binbag.h"
#include "owndebug.h"
#include <assert.h>

#define GIMI_CLIENT_API (20000)
#include "gimi.h"


//*****************************************************************************
//*****************************************************************************
using namespace MaCI::Ranging;
//*****************************************************************************
//*****************************************************************************

CRangingServer::CRangingServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                               const int aInterfaceMinor,
                               const std::string &aDescription)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_RANGING, aInterfaceMinor),
    CSync(0,1),
    CThread(1),
    iDistanceArrayEventSeq(0),
    iMessageSeq(0),
    iDevicePosition(),
    iDeviceLimits(),
    iServiceDescription(aDescription)
{
  dPrint(4,"Constructing CRangingServer instance %p", this);
  // Set Sync limits.
  SetInterfaceSubscriberSyncLimits(2, 1000);
}
//*****************************************************************************

CRangingServer::~CRangingServer()
{
  dPrint(4,"Destructing CRangingServer instance %p", this);
  DoClose();
  dPrint(4,"CRangingServer instance %p destroyed", this);
}
//*****************************************************************************

MaCI::EMaCIError CRangingServer::DoOpen(void)
{
  MaCI::EMaCIError result = KMaCIError;

  Lock();
  if (IsThreadRunning() == false) {
    RegisterProvidedService("MaCI-RangingServer; '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-RangingServerCommands");

    // Mark server active, and start thread.
    SetActive(true);
    CThread::RunThread();
    
    // Set result.
    result = KMaCIOK;

  } else {
    dPrint(ODWARN,"Tried to DoOpen() Device while Thread is still running!");

  }
  Unlock();

  return result;
}
//*****************************************************************************

MaCI::EMaCIError CRangingServer::DoClose()
{
  MaCI::EMaCIError result = KMaCIError;

  Lock();
  if (IsThreadRunning() == true) {
    // Mark server active flag false.
    SetActive(false);

    // Unregister 
    UnregisterProvidedService();
    UnregisterAcceptedService();
    
    // Cancel & Wait.
    dPrint(ODTEST,"Waiting for RangingServer thread..");
    Unlock();
    CThread::WaitThread();
    Lock();
    dPrint(ODTEST,"Wait complete.");

    result = KMaCIOK;

  } else {
    dPrint(ODWARN,"Tried to DoClose() device while Thread is not running!");

  }
  Unlock();

  return result;
}
//*****************************************************************************

bool CRangingServer::SendDistanceArrayEvent(const CRangingData &aData)
{
  bool result = false;

  assert(iGIMIPtr != NULL);

  if (aData.IsValid()) {
    SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                   aData.GetBinBagContainerPtr()->GetBufferSize(),
                                   ++iDistanceArrayEventSeq);
    result = true;
    dPrint(10,"SendInterfaceSubscriberMessage called: %d:%d, seq: %d (%d bytes of payload sent)",
           iInterfaceMajor, iInterfaceMinor, iDistanceArrayEventSeq-1,
           aData.GetBinBagContainerPtr()->GetBufferSize());
  
  } else {
    dPrint(1,"Invalid DistanceArrayEvent data. Not calling SendToSubscribers");
    
  }
  
  return result;
}
//*****************************************************************************

void CRangingServer::SetDeviceLimits(const float aMinDistance, const float aMaxDistance)
{
  // Set new limits
  Lock();
  iDeviceLimits = TDeviceInformationLimits( aMaxDistance, aMinDistance);
  Unlock();
}
//*****************************************************************************
      
void CRangingServer::SetDevicePosition(const float aX, const float aY, 
                                       const float aZ, const float aHeading)
{
  // Set new position
  Lock();
  iDevicePosition = TDeviceInformationPosition(aX, aY, aZ, aHeading);
  Unlock();
}
//*****************************************************************************
//*****************************************************************************

int CRangingServer::ThreadFunction(int aThreadNumber)
{
  int p = 0;
  int result = 0;
  gimi::GIMIMessage gmsg;

  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);

  dPrint(ODTEST,"ThreadFunction started");

  // Run as long as the thread should be active
  while(IsActive()) {
    EMaCIError err = ReceiveInterfaceMessage(gmsg, 5000);
    switch(err) {
    case KMaCIOK:
      // Got GIMI OK, so process message.
      p = ProcessMessage(gmsg);
      break;

    case KMaCITimeout:
      // GIMI Timeout - Do nothing.
      break;
      
    default:
      // Any error, print warning and sleep for one second.
      dPrint(ODWARN, "Error from ReceiveInterfaceMessage received. Ignore.");
      break;
    }
  }

  dPrint(ODTEST,"ThreadFunction terminated");

  return result;
}
//*****************************************************************************

int CRangingServer::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CRangingData rd;
  bool r;

  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    r = rd.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as Ranging
      // Now, Check whether it was Command, if so, process it and construct reply
      gim::binbag::CBinBag replybb;
      CRangingData reply;
      bool send_reply = false;

      // Do encoding to this binbag
      reply.EncodeTo(&replybb);

      Lock();

      const TCommand *cmd = NULL;
      const int max_cmd = rd.GetCommandCount();
      for(int i=0; i < max_cmd; ++i) {
        cmd = rd.GetCommand(i);
        if (cmd) {
          dPrint(8,"Got command: '%d'", cmd->cmd);
          switch(cmd->cmd) {
          case KCommandQueryPosition:
            reply.SetDeviceInformationPosition(iDevicePosition);
            send_reply = true;
            break;

          case KCommandQueryLimits:
            reply.SetDeviceInformationLimits(iDeviceLimits);
            send_reply = true;
            break;

          default:
            dPrint(2,"Unsupported Ranging command %d received. Ignoring.",
                   cmd->cmd);
            break;
          }
        }
      }

      Unlock();

      // Check whether reply needs to be sent
      if (send_reply) {
        reply.SetTimestamp(Common::TTimestamp());
        SendSynchronousMessageReply(replybb.GetBuffer(),
                                    replybb.GetBufferSize(),
                                    msg);
      }
    }
  }
  
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
